#include "uart.h"
#include "stm32f10x.h"
#include <stdlib.h>
#include <stdio.h>
#include <errno.h>
#include <string.h>
#include "MISE_protocol.h"
#include "Servo.h"
#include "Power.h"


//TODO
float dummy[10];

volatile int16_t speed = 0;
volatile float motor_p=2.0;
volatile float motor_i=3.8;
volatile float motor_d=0;
volatile uint8_t motor_taskdelay=100;

#ifdef ARCADE

volatile float servo_p_o[INTERP_SERVO_PARAMS_SIZE]={1.5,1.5F};
volatile float servo_i_o[INTERP_SERVO_PARAMS_SIZE]={0.0F,0.0F};
volatile float servo_d_o[INTERP_SERVO_PARAMS_SIZE]={0.0F,0.0F};
volatile float servo_ratio_o[INTERP_SERVO_PARAMS_SIZE]={0.5F,0.5F};

volatile float servo_p_v[INTERP_SERVO_PARAMS_SIZE]={0.8F,0.0F};
volatile float servo_i_v[INTERP_SERVO_PARAMS_SIZE]={0.0F,0.0F};
volatile float servo_d_v[INTERP_SERVO_PARAMS_SIZE]={0.2F,0.0F};
volatile float servo_ratio_v[INTERP_SERVO_PARAMS_SIZE]={1.0F,0.0F};

volatile uint8_t servo_taskdelay=6;
#endif

#ifdef SPEEDY

volatile int16_t speed_slow=150;
volatile int16_t speed_fast=350;
volatile float servo_p_o[INTERP_SERVO_PARAMS_SIZE]={1.5F,1.0F};
volatile float servo_i_o[INTERP_SERVO_PARAMS_SIZE]={0.0F,0.0F};
volatile float servo_d_o[INTERP_SERVO_PARAMS_SIZE]={0.0F,4.0F};
volatile float servo_ratio_o[INTERP_SERVO_PARAMS_SIZE]={1.0F,1.0F};

volatile float servo_p_v[INTERP_SERVO_PARAMS_SIZE]={0.5F,0.3F};
volatile float servo_i_v[INTERP_SERVO_PARAMS_SIZE]={0.0F,0.0F};
volatile float servo_d_v[INTERP_SERVO_PARAMS_SIZE]={0.0F,0.0F};
volatile float servo_ratio_v[INTERP_SERVO_PARAMS_SIZE]={1.0F,1.0F};
volatile uint8_t servo_taskdelay=6;


volatile uint8_t cascademultiplier=0;
#endif

void MISE_Interpreter(uint16_t Type, uint8_t * Data, uint16_t Length)
{

	switch( Type ){
		case 13:
			//TODO
			break;
		case 54:{
			int i = 0;
			for(i=0;i<Length;i+=3){
				Servo_Set_Position(Data[i],Data[i+1],Data[i+2]);
			}
			 break;
		}		case 52:{
			 if(Data[0]==1)
				 Power_Servo_on();
			 else
				 Power_Servo_off();
			 break;
		}
		case 22:
			if (Data[0]==255)
				speed++;//INCREM_SetControlMode(INCREM_SPEED_CONTROLL);
			else
			{
				speed=(Data[0]>100)?-(Data[0]-100)*10:Data[0]*10;
				//INCREM_SetControlMode(INCREM_GUI_CONTROLL);
			}
			break;
		case 3:
		{
			float temp;
			temp= ((float)Data[0])/10;
			motor_p=temp;
			temp = ((float)Data[1])/5*2;
			motor_i=temp;
			temp = ((float)Data[2])/10;
			motor_d=temp;

			motor_taskdelay=Data[3];

			break;
		}

		case 4:
		{
			uint8_t index=0;
			index= Data[9];

			float temp;
		/*	temp= ((float)Data[0])/10;
			servo_p_v[index]=temp;
			temp= ((float)Data[1])/10;
			servo_i_v[index]=temp;
			temp= ((float)Data[2])/10;
			servo_d_v[index]=temp;

			temp= ((float)Data[3])/100;
			servo_ratio_v[index]=temp;


			temp= ((float)Data[4])/10;
			servo_p_o[index]=temp;
			temp= ((float)Data[5])/10;
			servo_i_o[index]=temp;
			temp= ((float)Data[6])/10;
			servo_d_o[index]=temp;

			temp= ((float)Data[7])/100;
			servo_ratio_o[index]=temp;


			servo_taskdelay=Data[8];*/
			break;
		}

#ifdef SPEEDY
		case MESSAGE_SPEED_SELECTOR:
			if (Data[0]<101)
				speed_slow=Data[0]*10;

			if (Data[1]<101)
				speed_fast=Data[1]*10;

			break;

		case MESSAGE_CASCADE_MULTIPLIER:
			if (Data[0]>0 && Data[0]<10)
				cascademultiplier=Data[0];

			break;
#endif
		default:
			break;
	}

	
}


int16_t INTERP_GetSpeed()
{
	volatile int16_t s=speed;
	return s;
}



float INTERP_GetRatioO(uint8_t index)
{
	return dummy[index];
}


float INTERP_GetServoPO(uint8_t index)
{
	return dummy[index];
}

float INTERP_GetServoIO(uint8_t index)
{
	return dummy[index];
}

float INTERP_GetServoDO(uint8_t index)
{
	return dummy[index];
}

float INTERP_GetRatioV(uint8_t index)
{
	return dummy[index];
}


float INTERP_GetServoPV(uint8_t index)
{
	return dummy[index];
}

float INTERP_GetServoIV(uint8_t index)
{
	return dummy[index];
}

float INTERP_GetServoDV(uint8_t index)
{
	return dummy[index];
}



float INTERP_GetMotorP()
{
	return motor_p;
}

float INTERP_GetMotorI()
{
	return motor_i;
}

float INTERP_GetMotorD()
{
	return motor_d;
}


uint8_t INTERP_GetServoTask()
{
	return 5;
}

uint8_t INTERP_GetMotorTask()
{
	return motor_taskdelay;
}

#ifdef SPEEDY
inline int16_t INTERP_GetSpeedFast()
{
	return speed_fast;
}

inline int16_t INTERP_GetSpeedSlow()
{
	return speed_slow;
}

inline uint8_t INTERP_GetCascadeMultuplier()
{
	return cascademultiplier;
}
#endif
